/*
 * CarCode.c
 * 
 * Currently just having a play with the AVR studio.
 * Created: 18/07/2011 11:46:16 a.m.
 */ 

#include <avr/io.h>

#define BIT(x)        (1 << (x))
#define ISSET(a,x)    ((a) & BIT((x)) ? 1 : 0 )

#define LEFT_MOTOR_ENABLE PB0	
#define LMF PB1
#define LMR PB2
#define RIGHT_MOTOR_ENABLE PB5
#define RMF PB3
#define RMR PB4

#define S1 PC0   //ADC0
#define S2 PC1   //ADC1


#define MOTOR_MASK ( BIT(LEFT_MOTOR_ENABLE) | BIT(LMF) | BIT(LMR) | BIT(RMF) | BIT(RMR) | BIT(RIGHT_MOTOR_ENABLE) )
#define SENSOR_MASK ( BIT(S1) | BIT(S2) )

//The Nember of sensors the robot is using, changes mode.
#define NUM_SENSORS 3


uint_16 sampleADC(int sensor_number);
int readSensors();
int operateMotors(int leftMotor, int rightMotor);
void initialiseMotors();
void initialiseSensors();


int main(void)
{
	initialiseSensors();
	initialiseMotors();
	
    while(1)
    {

       //Read sensors
	   
	   //compute necessary action
			#if NUM_SENSORS == 3
				//Code to run robot on 3 sensors
			#elif NUM_SENSORS == 2
				//Code to run robot on 2 sensors
			#else
				//Only using 2 or 3 sensors config so report error.
				printf("Invalid operation mode, %i, selected in file %s near line %d\n", NUM_SENSORS, __FILE__, __LINE__);
			#endif


	   
	   //operate motors. 
    }
}

readSensors()
{
	//reading pinC
	//read s1
	//read s2
}

operateMotors()
{
	//if black
	//if white
	//driving pinB
}

int initialiseSensors()	
{
	DDRC &= ~SENSOR_MASK;  //already set as inputs
	PORTC |= SENSOR_MASK;  //pull up
	
}

int initialiseMotors()
{
	DDRB |= MOTOR_MASK;   //set to 1, setting to outputs
}